home *** CD-ROM | disk | FTP | other *** search
/ SuperHack / SuperHack CD.bin / CODING / MISC / AFIR22.ZIP / AFIR22.ASM next >
Encoding:
Assembly Source File  |  1993-08-10  |  7.6 KB  |  254 lines
  1. ;***********************************************************************
  2. ;
  3. ;        FINITE IMPULSE RESPONSE FOR DSP-12
  4. ;        Least Mean Squares (LMS) algorithm.
  5. ;
  6. ;    Based on "LMS Autonotcher/Denoiser" of Jarkko Vuori (OH2LNS)
  7. ;
  8. ;    Version : 2.2
  9. ;    Fecha   : 28/Febrero/1993
  10. ;    Autores : Moises Zafra (EA4QV/EA4MZ) / Fernando Limon (EA8SU/EA4SU)
  11. ;
  12. ;***********************************************************************
  13.     include '..\asm\ioequ'
  14.     include '..\asm\intequ'
  15.  
  16. ;-----------------------------------------------------------------------
  17. ; Internal equates
  18. ;-----------------------------------------------------------------------
  19. IO_D2A    equ    $FFC0        ;D-to-A data
  20. IO_A2D    equ    $FFC4        ;A-to-D data
  21. IO_FLATCH equ    $FFC8        ;filter latch
  22. IO_8254    equ    $FFD0        ;programmable timer
  23.  
  24. ;-----------------------------------------------------------------------
  25. ; Maxim filters coefficients
  26. ;-----------------------------------------------------------------------
  27. maxcoef
  28.     dc    $C0
  29.     dc    $F1
  30.     dc    $C2
  31.     dc    $C3
  32.     dc    $F4
  33.     dc    $D5
  34.     dc    $F6
  35.     dc    $C7
  36.     dc    $C8
  37.     dc    $F9
  38.     dc    $CA
  39.     dc    $CB
  40.     dc    $FC
  41.     dc    $DD
  42.     dc    $FE
  43.     dc    $CF
  44. ;
  45.  
  46. lmslen    equ    24        ;LMS filter lenght
  47. buflen    equ     8        ;lenght of sample buffer
  48.  
  49.     if    notch
  50. ; Notcher constants
  51. dlen    equ    63        ;Delay line lenght
  52. beta    equ    0.125        ;Adaptation coefficient
  53. decay    equ    0.99915        ;Coefficient decay value
  54.     else
  55. ; Denoiser constants
  56. dlen    equ    1        ;Delay line lenght
  57. beta    equ    0.1875        ;Adaptation coefficient
  58. decay    equ    0.98047        ;Coefficient decay value
  59.     endif
  60.  
  61. g    equ    0.007        ;Biquad filter scaler
  62.  
  63.     org    x:$0
  64. iirs    ds    2
  65. buffer    dsm    buflen*4
  66. dline    dsm    dlen+lmslen
  67.  
  68.     org    y:$0
  69. iircoef    dc    -1.880563819/2.0, 0.8990682309/2.0
  70.     dc    -1.999905221/2.0, 1.0/2.0
  71.     dsm    buflen*4
  72. lmscoef    dsm    lmslen
  73.  
  74. ;-----------------------------------------------------------------------
  75. ; Interrupt vectors
  76. ;-----------------------------------------------------------------------
  77.     org    p:I_RESET               ;Entry point
  78.     jmp    main
  79.  
  80.     org    p:I_IRQA                ;Sample interrupt
  81.     jsr    sample
  82.  
  83.  
  84. ;***********************************************************************
  85. ; MAIN CODE
  86. ;***********************************************************************
  87.     org    p:$100
  88. main
  89.  
  90. ;------------------------------------------------------------------------------
  91. ; Program the BCR as reqd
  92. ;------------------------------------------------------------------------------
  93.     movep    #>$0003,x:M_BCR        ;No waits for memory, 3 for i/o
  94.  
  95. ;------------------------------------------------------------------------------
  96. ; NOTE: The 82C54 input clock is 3.9936 MHZ
  97. ; Program TIMER-0: RX filter clock
  98. ;------------------------------------------------------------------------------
  99.     movep    #>$36,y:IO_8254+3    ;Mode 3, LSB+MSB, binary
  100.     movep    #>$14,y:IO_8254+0    ;3,993,600 / 20 = about 200khz
  101.     movep    #>$00,y:IO_8254+0
  102.  
  103. ;------------------------------------------------------------------------------
  104. ; Program TIME-1: TX filter clock
  105. ;------------------------------------------------------------------------------
  106.     movep    #>$76,y:IO_8254+3    ;Mode 3, LSB+MSB, binary
  107.     movep    #>$14,y:IO_8254+1    ;3,993,600 / 20 = about 200khz
  108.     movep    #>$00,y:IO_8254+1
  109.  
  110. ;------------------------------------------------------------------------------
  111. ; Program TIMER-2: sample clock
  112. ;------------------------------------------------------------------------------
  113.     movep    #>$B4,y:IO_8254+3    ;Mode 2, LSB+MSB, binary
  114.     movep    #>$2B,y:IO_8254+2    ;3,993,600 / 555 = about 7.2k samples/sec
  115.     movep    #>$02,y:IO_8254+2
  116.  
  117. ;------------------------------------------------------------------------------
  118. ; Program the MAXIM filters
  119. ; Both Filters: Mode=1, FN=3, QN=55
  120. ;------------------------------------------------------------------------------
  121.     move    #>$C0,x0        ;eor bit for both tx and rcv filter
  122.     move    #maxcoef,r1
  123.     do    #16,end_f
  124.     move    x:(r1)+,a1
  125.     movep    a1,y:IO_FLATCH
  126.     eor    x0,a
  127.     movep    a1,y:IO_FLATCH
  128.     eor    x0,a
  129.     movep    a1,y:IO_FLATCH
  130. end_f    nop
  131.  
  132. ;------------------------------------------------------------------------------
  133. ; Init structure for data and arrange for interrupts as reqd
  134. ;------------------------------------------------------------------------------
  135.     move    #<buffer+2,r7        ;Interrupt handler pointer
  136.     move    #buflen*4-1,m7
  137.  
  138.     move    #<lmscoef,r6        ;LMS filter coeff pointer
  139.  
  140.     move    #<iircoef,r5        ;IIR filter coeff pointer
  141.     move    #4-1,m5
  142.  
  143.     move    #<buffer,r2        ;Sample buffer read pointer
  144.     move    #4-1,n2
  145.     move    #buflen*4-1,m2
  146.  
  147.     move    #dline,r1        ;Delay line sample pointer
  148.     move    #dlen+lmslen-1,m1
  149.  
  150.     movep    #>$0007,x:M_IPR        ;Program IRQ-A to be edge triggered, 
  151.                     ;level 2
  152.     move    #0,sr            ;Allow all interrupts
  153.  
  154. loop    jmp    loop            ;Infinite loop
  155.  
  156. ;***********************************************************************
  157. ; INTERRUPT HANDLER
  158. ;***********************************************************************
  159. sample
  160.  
  161. ;-----------------------------------------------------------------------
  162. ; Read from A/D
  163. ;-----------------------------------------------------------------------
  164.     movep    y:IO_A2D,x0            ; read A/D
  165.     move    x0,y:(r2)+
  166.  
  167. ;-----------------------------------------------------------------------
  168. ; Highpass filter the input signal (with one biquad IIR section)
  169. ;-----------------------------------------------------------------------
  170.     ori    #$0B,mr                ;Set left shift scaling mode
  171.     move    #g,x1                ;Scale input signal
  172.     mpy    x0,x1,a        #<iirs,r0
  173.     nop
  174.  
  175.     move            x:(r0)+,x0    y:(r5)+,y0    ;s1, a1
  176.     mac    -x0,y0,a    x:(r0),x1    y:(r5)+,y0    ;s2, a2
  177.     macr    -x1,y0,a    x0,x:(r0)-    y:(r5)+,y0    ;new s2, get b1
  178.     mac    x0,y0,a        a,x:(r0)    y:(r5)+,y0    ;new s1, get b2
  179.     macr    x1,y0,a
  180.     andi    #$F4,mr                ;Restore scaling mode
  181.  
  182.     move    a,x0                ;Back scaling
  183.     move    #>@cvi(1.0/g)-5,x1
  184.     mpy    x0,x1,a
  185.     asr    a
  186.     move    a0,x:(r1)+
  187.     move    a0,x1
  188.  
  189. ;-----------------------------------------------------------------------
  190. ; FIR filter
  191. ;-----------------------------------------------------------------------
  192.     clr    a        x:(r1)+,x0    y:(r6)+,y0
  193.     rep    #lmslen-1
  194.     mac    x0,y0,a        x:(r1)+,x0    y:(r6)+,y0
  195.     macr    x0,y0,a        x:(r1)-,x0    y:(r6)-,y0
  196.  
  197. ;-----------------------------------------------------------------------
  198. ; Store FIR output as a program output if denoiser code
  199. ;-----------------------------------------------------------------------
  200.     if    !notch
  201.     move    a,y:(r2)+n2
  202.     endif
  203.  
  204. ;-----------------------------------------------------------------------
  205. ; Calculate error (e = D - Y) to x1
  206. ;-----------------------------------------------------------------------
  207.     neg    a
  208.     add    x1,a
  209.     move    a,x1
  210.  
  211. ;-----------------------------------------------------------------------
  212. ; Store error as a program output if notcher code
  213. ;-----------------------------------------------------------------------
  214.     if    notch
  215.     move    x1,y:(r2)+n2
  216.     endif
  217.  
  218. ;-----------------------------------------------------------------------
  219. ; Write to D/A
  220. ;-----------------------------------------------------------------------
  221. write
  222.     move    a,x0
  223.     move    y:(r2)+n2,a
  224.     move    #>$800000,y1
  225.     eor    y1,a
  226.     neg    a
  227.     movep    a1,y:IO_D2A
  228.     move    x0,a
  229.  
  230. ;-----------------------------------------------------------------------
  231. ; Wiener filter adaptation part
  232. ;-----------------------------------------------------------------------
  233.     move    #beta,x0
  234.     move    #decay,y1
  235.  
  236.     mpyr    x0,x1,a        r6,r4                ;x1=beta*e
  237.     move    a,x1
  238.  
  239.     move            x:(r1)-,x0    y:(r6)-,y0    ;get x(0), c(0)
  240.     mpy    y0,y1,a
  241.     macr    x0,x1,a        x:(r1)-,x0    y:(r6)-,y0    ;c(0)=decay*c(0)
  242.     do    #lmslen-1,adaloop                ; + e*x(0)
  243.     mpy    y0,y1,a                a,y:(r4)-    ;save new c(0)
  244.     macr    x0,x1,a        x:(r1)-,x0    y:(r6)-,y0    ;c(n)=decay*c(n)
  245. adaloop                                ; + e*x(n)
  246.     move                    a,y:(r4)-    ;save new c(n)
  247.     move            x:(r1)+,x0    y:(r6)+,y0
  248.     move            x:(r1)+,x0    y:(r6)+,y0
  249.  
  250. ;-----------------------------------------------------------------------
  251.     rti
  252.  
  253.     end    main
  254.